import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.addon.CompassSensor;
import lejos.robotics.navigation.CompassPilot;
import lejos.robotics.navigation.Pilot;

public class Project1 {

	Pilot pilot;
	public final static int DELAY_SHORT = 50;
	public final static int DELAY_LONG = 100;
	public final static float STEP_DISTANCE = 1f;
	public final static float TURN_DISTANCE = 2f;
	public final static int REQUIRED_TURNING_LIGHT_DIFFERENCE = 15;

	/**
	 * Moves robot forward a bit.
	 * @param float
	 */
	public void moveForward(float distance) {
		pilot.reset();
		pilot.travel(distance);
	}
	
	/**
	 * Takes a step forward.
	 */
	public void stepForward() {
		this.moveForward(STEP_DISTANCE);
	}
	
	public void turnLeft() {
		pilot.rotate(-1 * TURN_DISTANCE);
	}

	public void turnRight() {
		pilot.rotate(TURN_DISTANCE);
	}

	public static void main(String[] args) {
		Project1 traveler = new Project1();
		LightSensor leftLightSensor, rightLightSensor;
		int rightValue, leftValue, whiteValue;
		
		// Initialize light sensors
		leftLightSensor = new LightSensor(SensorPort.S1, false);
		rightLightSensor = new LightSensor(SensorPort.S4, false);

		// Wheel Diameter: 3.6cm
		// Track Width: 13cm
		CompassPilot pilot = new CompassPilot(new CompassSensor(SensorPort.S3),
				3.6f, 13f, Motor.C, Motor.A);
		traveler.pilot = pilot;
		((CompassPilot)(traveler.pilot)).setSpeed(500);

		System.out.println("Ready...");
		leftLightSensor.setFloodlight(true);
		Button.waitForPress(DELAY_SHORT);
		System.out.println("   ...set...");
		rightLightSensor.setFloodlight(true);
		Button.waitForPress(DELAY_SHORT);
		System.out.println("      ...go!!!");
		Button.waitForPress(DELAY_SHORT);

		LCD.clear();
		whiteValue = (leftLightSensor.getLightValue() + rightLightSensor.getLightValue()) / 2;
		boolean leftIsWhite = true;
		boolean rightIsWhite = true;
		boolean needToRecheck;
		for (int i = 0; i < 1000; i++) {
			Button.waitForPress(DELAY_LONG);
			traveler.stepForward();
			leftValue = leftLightSensor.getLightValue();
			leftIsWhite = whiteValue - leftValue < 10;
			rightValue = rightLightSensor.getLightValue();
			rightIsWhite = whiteValue - rightValue < 10;
			System.out.println("W/L/R: " + whiteValue + "/" + leftValue + "/" + rightValue);
			needToRecheck = false;
			while (needToRecheck || !leftIsWhite || !rightIsWhite) {
				System.out.println("Adjusting...");
				needToRecheck = false;
				leftValue = leftLightSensor.getLightValue();
				leftIsWhite = whiteValue - leftValue < 10;
				rightValue = rightLightSensor.getLightValue();
				rightIsWhite = whiteValue - rightValue < 10;
				System.out.println("W/L/R: " + whiteValue + "/" + leftValue + "/" + rightValue);
				while (!leftIsWhite) {
					traveler.turnLeft();
					leftValue = leftLightSensor.getLightValue();
					leftIsWhite = whiteValue - leftValue < REQUIRED_TURNING_LIGHT_DIFFERENCE;
					System.out.println("W/L: " + whiteValue + "/" + leftValue);
					needToRecheck = true;
					Button.waitForPress(DELAY_SHORT);
				}
				while (!rightIsWhite) {
					traveler.turnRight();
					rightValue = rightLightSensor.getLightValue();
					rightIsWhite = whiteValue - rightValue < REQUIRED_TURNING_LIGHT_DIFFERENCE;
					System.out.println("W/R: " + whiteValue + "/" + rightValue);
					needToRecheck = true;
					Button.waitForPress(DELAY_SHORT);
				}
			}
		}
		
		// Pause for 60 seconds with output on display
		System.out.println("Done.");
		Button.waitForPress(60000);
	}
}
